#!/usr/bin/env python

PACKAGE='costmap_2d_custom'
import roslib; roslib.load_manifest(PACKAGE)

import sys
from math import pi

from dynamic_reconfigure.parameter_generator import *

gen = ParameterGenerator()

enum = gen.enum([ gen.const("voxel_const", str_t, "voxel", "Use VoxelCostmap2D"), gen.const("costmap_const", str_t, "costmap", "Use Costmap2DCustom")], "An enum to set the map type")

gen.add("transform_tolerance", double_t, 0, "Specifies the delay in transform (tf) data that is tolerable in seconds.", 0.3, 0, 10)

gen.add("update_frequency", double_t, 0, "The frequency in Hz for the map to be updated.", 5, 0, 100)
gen.add("publish_frequency", double_t, 0, "The frequency in Hz for the map to be publish display information.", 0, 0, 100)

gen.add("max_obstacle_height", double_t, 0, "The maximum height of any obstacle to be inserted into the costmap in meters.", 2, 0, 50)
gen.add("max_obstacle_range", double_t, 0, "The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 2.5, 0, 50)
gen.add("raytrace_range", double_t, 0, "The default range in meters at which to raytrace out obstacles from the map using sensor data.", 3, 0, 50)
gen.add("cost_scaling_factor", double_t, 0, "A scaling factor to apply to cost values during inflation.", 10, 0, 100)

gen.add("inflation_radius", double_t, 0, "The radius in meters to which the map inflates obstacle cost values.", 0.55, 0, 50)
gen.add("footprint", str_t, 0, "The footprint of the robot specified in the robot_base_frame coordinate frame as a list in the format: [ [x1, y1], [x2, y2], ...., [xn, yn] ].", "[]");
gen.add("robot_radius", double_t, 0, 'The radius of the robot in meters, this parameter should only be set for circular robots, all others should use the footprint parameter described above.', 0.46, 0, 10)

gen.add("static_map", bool_t, 0, "Whether or not to use the static map to initialize the costmap.", True)
gen.add("rolling_window", bool_t, 0, "Whether or not to use a rolling window version of the costmap.", False)
gen.add("unknown_cost_value", int_t, 0, "The value for which a cost should be considered unknown when reading in a map from the map server.", 0, 0, 255)

#map params
gen.add("width", int_t, 0, "The width of the map in meters.", 0, 0, 20)
gen.add("height", int_t, 0, "The height of the map in meters.", 10, 0, 20)
gen.add("resolution", double_t, 0, "The resolution of the map in meters/cell.", 0.05, 0, 50)
gen.add("origin_x", double_t, 0, "The x origin of the map in the global frame in meters.", 0, 0) 
gen.add("origin_y", double_t, 0, "The y origin of the map in the global frame in meters.", 0, 0)

gen.add("publish_voxel_map", bool_t, 0, "Whether or not to publish the underlying voxel grid for visualization purposes.", False)
gen.add("lethal_cost_threshold", int_t, 0, "The threshold value at which to consider a cost lethal when reading in a map from the map server.", 100, 0, 255)
gen.add("map_topic", str_t, 0, "The topic that the costmap subscribes to for the static map.", "map")

#map type params
gen.add("map_type", str_t, 0, 'What map type to use. voxel or costmap are the supported types', "costmap", edit_method = enum)
gen.add("origin_z", double_t, 0, "The z origin of the map in meters.", 0, 0)
gen.add("z_resolution", double_t, 0, "The z resolution of the map in meters/cell.", 0.2, 0, 50)
gen.add("z_voxels", int_t, 0, "The number of voxels to in each vertical column.", 10, 0, 16)
gen.add("unknown_threshold", int_t, 0, 'The number of unknown cells allowed in a column considered to be known', 15, 0, 16)
gen.add("mark_threshold", int_t, 0, 'The maximum number of marked cells allowed in a column considered to be free', 0, 0, 16)
gen.add("track_unknown_space", bool_t, 0, "Specifies whether or not to track what space in the costmap is unknown", False)

gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration", False)

exit(gen.generate(PACKAGE, "costmap_2d_custom", "Costmap2DCustom"))
